#include <Arduino.h>

#include "esp_camera.h"
#include <WiFi.h>
#include "WebSocketsClient.h"

//
// WARNING!!! PSRAM IC required for UXGA resolution and high JPEG quality
//            Ensure ESP32 Wrover Module or other board with PSRAM is selected
//            Partial images will be transmitted if image exceeds buffer size
//

// Select camera model
//#define CAMERA_MODEL_WROVER_KIT // Has PSRAM
//#define CAMERA_MODEL_ESP_EYE // Has PSRAM
//#define CAMERA_MODEL_M5STACK_PSRAM // Has PSRAM
//#define CAMERA_MODEL_M5STACK_V2_PSRAM // M5Camera version B Has PSRAM
//#define CAMERA_MODEL_M5STACK_WIDE // Has PSRAM
//#define CAMERA_MODEL_M5STACK_ESP32CAM // No PSRAM
#define CAMERA_MODEL_AI_THINKER // Has PSRAM
//#define CAMERA_MODEL_TTGO_T_JOURNAL // No PSRAM

#include "camera_pins.h"
#include "config.h"
#include "action.h"

const char *ssid = WIFI_NAME;
const char *password = WIFI_PASSWORD;
WebSocketsClient webSocket;

#include "ESP32Servo.h"
#define PIN_SERVO 12 //定义舵机引脚
Servo servo;
const int minUs = 1000;
const int maxUs = 2000;

bool isOpenCamera;

//本地http服务器渲染
void startCameraServer();

void log(const char *format, ...) {
    Serial.printf(format);
}

void logln(const char *format, ...) {

}

bool initCamera() {
    camera_config_t config;
    config.ledc_channel = LEDC_CHANNEL_0;
    config.ledc_timer = LEDC_TIMER_0;
    config.pin_d0 = Y2_GPIO_NUM;
    config.pin_d1 = Y3_GPIO_NUM;
    config.pin_d2 = Y4_GPIO_NUM;
    config.pin_d3 = Y5_GPIO_NUM;
    config.pin_d4 = Y6_GPIO_NUM;
    config.pin_d5 = Y7_GPIO_NUM;
    config.pin_d6 = Y8_GPIO_NUM;
    config.pin_d7 = Y9_GPIO_NUM;
    config.pin_xclk = XCLK_GPIO_NUM;
    config.pin_pclk = PCLK_GPIO_NUM;
    config.pin_vsync = VSYNC_GPIO_NUM;
    config.pin_href = HREF_GPIO_NUM;
    config.pin_sscb_sda = SIOD_GPIO_NUM;
    config.pin_sscb_scl = SIOC_GPIO_NUM;
    config.pin_pwdn = PWDN_GPIO_NUM;
    config.pin_reset = RESET_GPIO_NUM;
    config.xclk_freq_hz = 20000000;
    config.pixel_format = PIXFORMAT_JPEG;

    // if PSRAM IC present, init with UXGA resolution and higher JPEG quality
    //                      for larger pre-allocated frame buffer.
    if (psramFound()) {
        config.frame_size = FRAMESIZE_UXGA;
        config.jpeg_quality = 10;
        config.fb_count = 2;
    } else {
        config.frame_size = FRAMESIZE_SVGA;
        config.jpeg_quality = 12;
        config.fb_count = 1;
    }

#if defined(CAMERA_MODEL_ESP_EYE)
    pinMode(13, INPUT_PULLUP);
    pinMode(14, INPUT_PULLUP);
#endif

    // camera init
    esp_err_t err = esp_camera_init(&config);
    if (err != ESP_OK) {
        Serial.printf("Camera init failed with error 0x%x", err);
        return false;
    }

    sensor_t *s = esp_camera_sensor_get();
    // initial sensors are flipped vertically and colors are a bit saturated
    if (s->id.PID == OV3660_PID) {
        s->set_vflip(s, 1); // flip it back
        s->set_brightness(s, 1); // up the brightness just a bit
        s->set_saturation(s, -2); // lower the saturation
    }
    // drop down frame size for higher initial frame rate
    s->set_framesize(s, FRAMESIZE_VGA);

#if defined(CAMERA_MODEL_M5STACK_WIDE) || defined(CAMERA_MODEL_M5STACK_ESP32CAM)
    s->set_vflip(s, 1);
    s->set_hmirror(s, 1);
#endif
    Serial.printf("Camera init success");
    return true;
}

void captureVideo() {
    camera_fb_t *fb = NULL;

    fb = esp_camera_fb_get();
    if (NULL == fb) {
        Serial.println("Camera capture failed");
    } else {
        webSocket.sendBIN(fb->buf, fb->len);
        esp_camera_fb_return(fb);
        fb = NULL;
    }
}

void initWifi() {
    WiFi.begin(ssid, password);
    while (WiFi.status() != WL_CONNECTED) {
        delay(500);
        Serial.print(".");
    }
    Serial.println("");
    Serial.println("WiFi connected");
}

void hexdump(const void *mem, uint32_t len, uint8_t cols = 16) {
    const uint8_t* src = (const uint8_t*) mem;
    Serial.printf("\n[HEXDUMP] Address: 0x%08X len: 0x%X (%d)", (ptrdiff_t)src, len, len);
    for(uint32_t i = 0; i < len; i++) {
        if(i % cols == 0) {
            Serial.printf("\n[0x%08X] 0x%08X: ", (ptrdiff_t)src, i);
        }
        Serial.printf("%02X ", *src);
        src++;
    }
    Serial.printf("\n");
}

void initServo() {
    int channel = ESP32PWM::timerAndIndexToChannel(2, 0);
    ESP32PWM::allocateTimer(2);
    servo.setPeriodHertz(50);
    servo.attach(PIN_SERVO, minUs, maxUs);
    Serial.printf("ESP32PWM channel %d", channel);
}

void turnTo(int degree) {
    if (degree < 0) degree = 0;
    if (degree > 180) degree = 180;
    servo.write(degree);
    delay(100);
}

void webSocketEvent(WStype_t type, uint8_t * payload, size_t length) {
    const char* message = reinterpret_cast<char *>(payload);
    switch(type) {
        case WStype_DISCONNECTED:
            Serial.printf("[WSc] Disconnected!\n");
            break;
        case WStype_CONNECTED:
            Serial.printf("[WSc] Connected to url: %s\n", payload);
            // send message to server when Connected
            webSocket.sendTXT("Uploader Connected");
            break;
        case WStype_TEXT:
            Serial.printf("[WSc] get text: %s\n", message);
            if (strcmp(message, ACTION_OPEN_CAMERA) == 0) {
                Serial.println("[WSc] action: open camera");
                isOpenCamera = true;
            }
            if (strcmp(message, ACTION_CLOSE_CAMERA) == 0) {
                Serial.println("[WSc] action: close camera");
                isOpenCamera = false;
            }
            if (strcmp(message, ACTION_TURN0) == 0) {
                Serial.println("[WSc] action: turn0");
                turnTo(0);
            }
            if (strcmp(message, ACTION_TURN90) == 0) {
                Serial.println("[WSc] action: turn90");
                turnTo(90);
            }
            if (strcmp(message, ACTION_TURN180) == 0) {
                Serial.println("[WSc] action: turn180");
                turnTo(180);
            }
            // send message to server
            // webSocket.sendTXT("message here");
            break;
        case WStype_BIN:
            Serial.printf("[WSc] get binary length: %u\n", length);
            hexdump(payload, length);
            // send data to server
            // webSocket.sendBIN(payload, length);
            break;
        case WStype_ERROR:
            Serial.printf("[WSc] ERROR : %s\n", payload);
            break;
        case WStype_FRAGMENT_TEXT_START:
            Serial.printf("[WSc] FRAGMENT_TEXT_START : %s\n", payload);
            break;
        case WStype_FRAGMENT_BIN_START:
            Serial.printf("[WSc] FRAGMENT_BIN_START: %s\n", payload);
            break;
        case WStype_FRAGMENT:
            Serial.printf("[WSc] FRAGMENT: %s\n", payload);
            break;
        case WStype_FRAGMENT_FIN:
            Serial.printf("[WSc] FRAGMENT_FIN: %s\n", payload);
            break;
    }
}

void initWebSocket() {
    Serial.println("initWebSocket");
    webSocket.begin(WEBSOCKET_HOST, WEBSOCKET_PORT, WEBSOCKET_URL);
    // event handler
    webSocket.onEvent(webSocketEvent);
    webSocket.setReconnectInterval(3000);
    webSocket.enableHeartbeat(15000, 3000, 2);
}

void setup() {
    Serial.begin(115200);
    Serial.println();

    initCamera();
    initWifi();
    initWebSocket();
    initServo();
    turnTo(0);
}

void loop() {
    webSocket.loop();
    if (webSocket.isConnected() && isOpenCamera) {
        captureVideo();
    }
}